#include "BMI088.h"
#include "BMI088_reg.h"
#include "QuaternionEKF.h"
#include "drivers/dev_pwm.h"
#include "dwt.h"
#include "rtthread.h"
#include "imu.h"
#include "user_lib.h"


#define INS_TASK_PERIOD         25

#define LOG_TAG              "imutask"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>


static rt_mutex_t bmi088_offset_mutex = RT_NULL;
static rt_thread_t ins_offset_thread = RT_NULL;

rt_uint8_t thread_flag=0;
INS_t INS;
IMU_Param_t IMU_Param;

const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};


uint32_t INS_DWT_Count = 0;
void bmi088_temp_ctrl(void){
    PIDCalculate(&BMI088_Data.imu_temp_pid, BMI088_Data.temperature, BMI088_Data.TempWhenCali);
    rt_pwm_set(imu_temp_pwm_dev,1,5000,BMI088_Data.imu_temp_pid.Output);
    //LOG_D("imu temp output:%f",BMI088_Data.temperature);
}


// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
static void InitQuaternion(float *init_q4)
{
    float acc_init[3]     = {0};
    float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
    float axis_rot[3]     = {0};       // 旋转轴
    // 读取100次加速度计数据,取平均值作为初始值
    for (uint8_t i = 0; i < 100; ++i)
    {
        BMI088_data_acquire(&BMI088_Data);
        acc_init[0] += BMI088_Data.acc[0];
        acc_init[1] += BMI088_Data.acc[1];
        acc_init[2] += BMI088_Data.acc[2];
        rt_thread_mdelay(1);
    }
    for (uint8_t i = 0; i < 3; ++i)
        acc_init[i] /= 100;
    Norm3d(acc_init);
    // 计算原始加速度矢量和导航系重力加速度矢量的夹角
    float angle = acosf(Dot3d(acc_init, gravity_norm));
    Cross3d(acc_init, gravity_norm, axis_rot);
    Norm3d(axis_rot);
    init_q4[0] = cosf(angle / 2.0f);
    for (uint8_t i = 0; i < 2; ++i)
        init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
}


void ins_offset_task(void *parameter)
{
    while (1) {
        if ((rt_mutex_take(bmi088_offset_mutex, RT_WAITING_FOREVER))==RT_EOK)
        {
            LOG_D("imu offset task start!\n");
            if (BMI088_Data.BMI088_ERORR_CODE ==BMI088_NO_ERROR)
            {
                    IMU_Param.scale[0] = 1;
                    IMU_Param.scale[1] = 1;
                    IMU_Param.scale[2] = 1;
                    IMU_Param.Yaw = 0;
                    IMU_Param.Pitch = 0;
                    IMU_Param.Roll = 0;
                    IMU_Param.flag = 1;

                    float init_quaternion[4] = {0};
                    InitQuaternion(init_quaternion);
                    IMU_QuaternionEKF_Init(init_quaternion,10, 0.001, 10000000, 1, 0);
                    // noise of accel is relatively big and of high freq,thus lpf is used
                    INS.AccelLPF = 0.0085;

                    rt_mutex_release(bmi088_offset_mutex);
                    LOG_D("imu mutex released!\n");
                    rt_thread_delete(ins_offset_thread);
            }
        }
    }
}

void ins(void){
    const float gravity[3] = {0, 0, 9.81f};
    float dt =DWT_GetDeltaT(&INS_DWT_Count);
    if(BMI088_Data.BMI088_ERORR_CODE == BMI088_NO_ERROR && BMI088_Data.cali_mode==0)
    {
                BMI088_data_acquire(&BMI088_Data);

                INS.Accel[0] = BMI088_Data.acc[0];
                INS.Accel[1] = BMI088_Data.acc[1];
                INS.Accel[2] = BMI088_Data.acc[2];
                INS.Gyro[0] = BMI088_Data.gyro[0];
                INS.Gyro[1] = BMI088_Data.gyro[1];
                INS.Gyro[2] = BMI088_Data.gyro[2];

                // // demo function,用于修正安装误差,可以不管,本demo暂时没用
                IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
                // // 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
                // //INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
                // //INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;


                // // 核心函数,EKF更新四元数
                IMU_QuaternionEKF_Update(INS.Gyro[0], INS.Gyro[1], INS.Gyro[2], INS.Accel[0], INS.Accel[1], INS.Accel[2], dt);

                rt_memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));

                // 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
                BodyFrameToEarthFrame(xb, INS.xn, INS.q);
                BodyFrameToEarthFrame(yb, INS.yn, INS.q);
                BodyFrameToEarthFrame(zb, INS.zn, INS.q);

                
                // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
                float gravity_b[3];
                EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
                float tmp = (float)INS.AccelLPF + dt;
                for (rt_uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波
                {
                    INS.MotionAccel_b[i] = ( (INS.Accel[i] - gravity_b[i]) * dt )/ (tmp) + (INS.MotionAccel_b[i] * INS.AccelLPF) / (tmp);
                }

                BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
                
                // 获取最终数据
                INS.Yaw = QEKF_INS.Yaw;
                INS.Pitch = QEKF_INS.Pitch;
                INS.Roll = QEKF_INS.Roll;
                INS.YawTotalAngle = QEKF_INS.YawTotalAngle;


                bmi088_temp_ctrl();
    }
}

void INS_Task(void *parameter)
{


    while (1)
    {

        if (rt_mutex_take(bmi088_offset_mutex, RT_WAITING_FOREVER)==RT_EOK)
        {
            thread_flag=1;
        }
        if (thread_flag==1)
        {
            
            ins();
        }
        rt_thread_mdelay(1);
    }
}









int imu_task_init(void){
    rt_uint8_t ret =RT_EOK;

    /* 创建一个动态互斥量 */
    bmi088_offset_mutex = rt_mutex_create("dmutex", RT_IPC_FLAG_PRIO);
    if (bmi088_offset_mutex == RT_NULL)
    {
        LOG_E("create dynamic mutex failed.\n");
        return -1;
    }

    ins_offset_thread = rt_thread_create("imu_offset", ins_offset_task, RT_NULL, 2048, 24, 10);
    if (ins_offset_thread != RT_NULL)
    {
        rt_thread_startup(ins_offset_thread);
    }
    else
    {
        ret = RT_ERROR;
    }
    
    /* 创建 imu task 线程 */
    rt_thread_t thread = rt_thread_create("imu_task", INS_Task, RT_NULL, 2048, INS_TASK_PERIOD, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("imu thread init success!\n");

    return ret;
}
INIT_APP_EXPORT(imu_task_init);

